/*
    Copyright 2021 codenocold codenocold@qq.com
    Address : https://github.com/codenocold/dgm
    This file is part of the dgm firmware.
    The dgm firmware is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    The dgm firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "encoder.h"
#include "spi.h"
#include "pwm_curr.h"
#include "usr_config.h"
#include "util.h"

tMT6816  MT6816;
tEncoder Encoder;

void ENCODER_init(void)
{
    // Init
    Encoder.raw                 = 0;
    Encoder.count_in_cpr        = 0;
    Encoder.count_in_cpr_prev   = 0;
    Encoder.shadow_count        = 0;
    Encoder.pos_cpr_counts      = 0;
    Encoder.vel_estimate_counts = 0;
    Encoder.pos                 = 0;
    Encoder.vel                 = 0;
    Encoder.phase               = 0;
    Encoder.phase_vel           = 0;
    Encoder.interpolation       = 0;

    int   encoder_pll_bw   = 2000;
    float bandwidth        = MIN(encoder_pll_bw, 0.25f * PWM_FREQUENCY);
    Encoder.pll_kp         = 2.0f * bandwidth;           // basic conversion to discrete time
    Encoder.pll_ki         = 0.25f * SQ(Encoder.pll_kp); // Critically damped
    Encoder.snap_threshold = 0.5f * CURRENT_MEASURE_PERIOD * Encoder.pll_ki;
}

bool ENCODER_sample(void)
{
    uint16_t tx[2] = {0x8300, 0x8400};
    uint16_t rx[2] = {0};

    NCS_RESET();
    HAL_SPI_TransmitReceive(&hspi1, (uint8_t*)&tx[0], (uint8_t*)&rx[0], 1, 100);
    NCS_SET();

    NCS_RESET();
    HAL_SPI_TransmitReceive(&hspi1, (uint8_t*)&tx[1], (uint8_t*)&rx[1], 1, 100);
    NCS_SET();

    /* Get 14bit data */
    MT6816.angle = ((rx[0] << 6) | (rx[1] >> 2));
    //MT6816.angle <<= 4;

    if (MT6816.check_err_count) {
        MT6816.check_err_count--;
    }

    return true;
}

void ENCODER_loop(void)
{
    if (ENCODER_sample()) {
        if (UsrConfig.encoder_dir == +1) {
            Encoder.raw = MT6816.angle;
        } else {
            Encoder.raw = (ENCODER_CPR - MT6816.angle);
        }
    }

    /* Linearization */
    int off_1      = UsrConfig.offset_lut[(Encoder.raw) >> 11];             // lookup table lower entry
    int off_2      = UsrConfig.offset_lut[((Encoder.raw >> 11) + 1) % 128]; // lookup table higher entry
    int off_interp = off_1 + ((off_2 - off_1) * (Encoder.raw - ((Encoder.raw >> 11) << 11)) >> 11); // Interpolate between lookup table entries
    int count = Encoder.raw - off_interp;
    //count = Encoder.raw;

    /*  Wrap in ENCODER_CPR */
    while (count > ENCODER_CPR)
        count -= ENCODER_CPR;
    while (count < 0)
        count += ENCODER_CPR;
    Encoder.count_in_cpr = count;

    /* Delta count */
    int delta_count           = Encoder.count_in_cpr - Encoder.count_in_cpr_prev;
    Encoder.count_in_cpr_prev = Encoder.count_in_cpr;
    while (delta_count > +ENCODER_CPR_DIV)
        delta_count -= ENCODER_CPR;
    while (delta_count < -ENCODER_CPR_DIV)
        delta_count += ENCODER_CPR;

    /* Add measured delta to encoder count */
    Encoder.shadow_count += delta_count;

    /* Run vel PLL */
    Encoder.pos_cpr_counts += CURRENT_MEASURE_PERIOD * Encoder.vel_estimate_counts;
    float delta_pos_cpr_counts = (float) (Encoder.count_in_cpr - (int) Encoder.pos_cpr_counts);
    while (delta_pos_cpr_counts > +ENCODER_CPR_DIV)
        delta_pos_cpr_counts -= ENCODER_CPR_F;
    while (delta_pos_cpr_counts < -ENCODER_CPR_DIV)
        delta_pos_cpr_counts += ENCODER_CPR_F;
    Encoder.pos_cpr_counts += CURRENT_MEASURE_PERIOD * Encoder.pll_kp * delta_pos_cpr_counts;
    while (Encoder.pos_cpr_counts > ENCODER_CPR)
        Encoder.pos_cpr_counts -= ENCODER_CPR_F;
    while (Encoder.pos_cpr_counts < 0)
        Encoder.pos_cpr_counts += ENCODER_CPR_F;
    Encoder.vel_estimate_counts += CURRENT_MEASURE_PERIOD * Encoder.pll_ki * delta_pos_cpr_counts;

    // align delta-sigma on zero to prevent jitter
    bool snap_to_zero_vel = false;
    if (ABS(Encoder.vel_estimate_counts) < Encoder.snap_threshold) {
        Encoder.vel_estimate_counts = 0.0f;
        snap_to_zero_vel            = true;
    }

    // run encoder count interpolation
    // if we are stopped, make sure we don't randomly drift
    if (snap_to_zero_vel) {
        Encoder.interpolation = 0.5f;
        // reset interpolation if encoder edge comes
    } else if (delta_count > 0) {
        Encoder.interpolation = 0.0f;
    } else if (delta_count < 0) {
        Encoder.interpolation = 1.0f;
    } else {
        // Interpolate (predict) between encoder counts using vel_estimate,
        Encoder.interpolation += CURRENT_MEASURE_PERIOD * Encoder.vel_estimate_counts;
        // don't allow interpolation indicated position outside of [enc, enc+1)
        if (Encoder.interpolation > 1.0f)
            Encoder.interpolation = 1.0f;
        if (Encoder.interpolation < 0.0f)
            Encoder.interpolation = 0.0f;
    }
    float interpolated_enc = Encoder.count_in_cpr - UsrConfig.encoder_offset + Encoder.interpolation;
    while (interpolated_enc > ENCODER_CPR)
        interpolated_enc -= ENCODER_CPR;
    while (interpolated_enc < 0)
        interpolated_enc += ENCODER_CPR;

    float shadow_count_f = Encoder.shadow_count;
    float turns          = shadow_count_f / ENCODER_CPR_F;
    float residual       = shadow_count_f - turns * ENCODER_CPR_F;

    /* Outputs from Encoder for Controller */
    Encoder.pos = turns + residual / ENCODER_CPR_F;
    UTILS_LP_MOVING_AVG_APPROX(Encoder.vel, (Encoder.vel_estimate_counts / ENCODER_CPR_F), 5);
    Encoder.phase     = (interpolated_enc * M_2PI * UsrConfig.motor_pole_pairs) / ENCODER_CPR_F;
    Encoder.phase_vel = Encoder.vel * M_2PI * UsrConfig.motor_pole_pairs;
}
